The present invention relates to a position controller provided with a compensation means for suppressing vibration produced in a mechanism driven by a motor when the motor controls a position by driving an object to be controlled and, in particular, it relates to a position controller capable of positioning with high accuracy in a short time when a low-frequency disturbance is large.
Generally, for position control, a position signal of an encoder and the like attached to a servomotor is differentiated to produce a speed feedback signal and speed control is performed, and based on a position feedback signal from a position detector such as a linear scale and the like attached to a movement section driven by the servomotor (or an encoder and the like attached to the servomotor), position control is performed. A block diagram of such a position control system is as shown in FIG. 9.
In FIG. 9, 901 denotes a position control section and a position control gain is Kp. 902 denotes a speed control section, 903 denotes a servomotor, and 904 denotes a movement section. Position deviation ep is determined by subtracting position feedback signal yf from position command yr outputted from a numerical value controller and speed command vr is determined by multiplying this position deviation by the position control gain Kp. Speed deviation ev is determined by subtracting speed feedback signal vf from this speed command vr, torque command (current command) Tr is determined based on said speed deviation ev in the speed control section 902, and the servomotor 903 and the movement section 904 are driven based on said torque command Tr.
A control method for a motor whereby position control is performed based on a signal from a position detector such as a linear scale attached to a movement section driven by a motor is known as a closed loop method. However, if a motor and a movement section are spring-connected and the rigidity of the connected portion is weak, the movement of the motor and movement section does not coincide and vibration occurs. In such a case, it is necessary to stabilize the loop by lowering a loop gain or to apply a method for stabilizing the loop, without lowering the loop gain, by correcting a torque command value based on a movement position, a movement speed and a motor position, or a motor speed. As a method for stabilizing the loop by correcting a torque command value, there is Japanese Unexamined Patent Publication No. Hei-3-110307, for example. Therein, the loop is stabilized by correcting the torque command value based on the difference between the movement section speed and motor speed. A method whereby the loop is stabilized by correcting the torque command value based on the difference between the movement section speed and motor speed has also been indicated in the literature and the like (for example in xe2x80x9cThe theory of AC servo system and practice of designxe2x80x9d written and edited by Hidehiko SUGIMOTO, Sougo-denshi Shuppansha.)
FIG. 8 is a block diagram for explaining the prior-art method whereby the loop is stabilized by correcting the torque command value.
In FIG. 8, 12 denotes a position control section in which position command Pref and detected movement position Pfbl are inputted and from which speed command Vref is outputted, and which position-controls a motor so that the abovementioned two input signals coincide, 13 denotes a speed control section in which the speed command Vref and operated motor speed Vfbm are inputted and from which torque command Terf is outputted, and which speed-controls a motor so that the abovedescribed two input signals coincide, 18 denotes a speed processor which receives motor position signal Pfbm as an output signal from a motor position detector to differentiate the signal and outputs the abovedescribed motor speed Vfbm, and 32 denotes a movement section speed process or which receives movement position signal Pfbl as an output signal from a movement position detector to differentiate the signal and outputs movement speed Vfbl. 21 denotes a subtracter which subtracts the movement position Pfbl from the position command Pref and outputs a position deviation signal, 22 denotes a position controller which receives the position deviation signal and outputs speed command Vref, 23 denotes a subtracter which subtracts the motor speed signal Vfbm from the speed command Vref and outputs a speed deviation, 24 denotes a speed controller which receives the speed deviation and outputs torque command, 25 denotes a subtracter which operates the difference between the movement speed signal Vfbl and the motor speed signal Vfbm to output, 28 denotes a coefficient multiplier which multiplies the difference between the abovedescribed movement speed signal Vfbl and the motor speed signal Vfbm by a coefficient xcex1 as a torque corrective gain and outputs the torque correction signal, and 31 denotes a subtracter which subtracts a torque correction signal from the torque command and outputs a new torque command Tref.
FIG. 10 is a system construction view to which the position control system is applied. 100 denotes a base, 101 denotes a ball screw, 102 denotes a table, 103 denotes a servomotor, 104 denotes a linear scale for detecting a position of the table, and 105 denotes a measuring head. The servomotor 103 drives the table 102 via the ball screw 101. All of the drive mechanism and non-movement portions of to-be-driven bodies are fixed on the same base 100 and the base 100 is installed on the ground. The main body of the linear scale 104 is arranged on the base 100 and the measuring head 105 is attached on the table 102. The control system position-controls the table 102 based on a position signal from the linear scale 104.
In recent years, the demand for higher speed industrial machines has increased, therefore a command is issued with an acceleration (deceleration) time made as short as possible (that is, by increasing acceleration (deceleration) as far as possible). In a case where the base 100 with a mechanism having a low rigidity is arranged on the ground, when the table is driven at high acceleration (deceleration), the base 100 receives the reaction force of the acceleration (deceleration) and vibrates severely. For the control system, the vibration of the base 100 becomes a disturbance signal of a position signal. In addition, the lower the rigidity of the combination mechanism between the servomotor 103 and the table 102 is, the greater the influence on positioning by the vibration of the base 100 becomes.
However in the abovedescribed prior art of FIG. 9, there has been a great drawback in that positioning with high accuracy is impossible in a short time when a large low-frequency disturbance exists. In the abovedescribed prior art of FIG. 8, there have been problems in that when low-frequency disturbance exists and minute vibration occurs, the phase of the torque correction signal and the phase of the vibration do not match each other, so the disturbance cannot be suppressed, thereby affecting the response. Also, since a speed signal is operated based on a position signal, there have been problems in that when the vibration is minute, sufficient resolution as a torque correction signal cannot be obtained and the loop cannot be stabilized. For example, since the speed signal is determined by differentiating the position signal, when the position signal is vibrating at a rate of several pulses, the waveform of the speed signal becomes coarse and the accuracy declines. Since the torque correction signal takes the difference between the coarse speed signals, its waveform becomes coarser and sufficient resolution in not obtainable.
Therefore, the present invention aims to provide a position controller capable of solving these problems. Disclosure of the Invention.
In order to solve the abovedescribed problems, according to the present invention, a position controller comprising: a position control section in which a position command and a detected movement position are inputted and from which a speed command is outputted; a speed control section in which the speed command and a motor speed are inputted and from which a torque command is outputted; a current control section which receives the torque command to perform amplification and outputs a current; a motor which rotates by being supplied with the current; a motor position detector which detects a rotational displacement of the shaft of the motor and outputs a motor position signal; a speed processor which receives the motor position signal and outputs a motor speed signal; and a movement position detector which detects the position of a movement section driven by the motor and outputs a movement position signal and provided with: a motor speed-control function which makes the speed command and the motor speed signal coincident by feedback control; and a motor position-control function which makes the position command and the movement position signal coincident by feedback control, wherein said position controller is provided with:
a subtracter which subtracts the motor position signal from the movement position signal and outputs an angle-of-twist signal; a high-pass filter which outputs the angle-of-twist signal with its phase advanced, upon receiving the angle-of-twist signal; a low-pass filter in which the signal from the high-pass filter is inputted and which outputs the signal with only its high-frequency components eliminated; a coefficient multiplier which receives the signal from the low-pass filter, multiplies it by a torque corrective gain, and outputs a torque correction signal; a section for torque correction evaluation which varies the torque corrective gain based on a position deviation signal that is the difference between the position command and the movement position signal; and a subtracter which subtracts the torque correction signal from the torque command to produce a new torque command, and further wherein:
in the section for torque correction evaluation, when the absolute value of the position deviation signal is greater than a reference value, the torque corrective gain is set to zero and when the absolute value of the position deviation signal changes to be equal to the reference value or below, the torque corrective gain is changed from zero to a constant value, or in the section for torque corrective gain changeover evaluation, when the absolute value of the position deviation signal is greater than a reference value, the torque corrective gain is set to zero and when the absolute value of the position deviation signal changes to be equal to the reference value or below, the torque corrective gain is continuously changed from zero to a constant value.
Also, according to the present invention, a position controller comprising: a position control section in which a position command and a movement position are inputted and from which a speed command is outputted; a speed control section in which the speed command and a motor speed are inputted and from which a torque command is outputted; a current control section which receives the torque command to perform amplification and outputs a current; a motor which rotates by being supplied with the current; a motor position detector which detects a rotational displacement of the shaft of the motor and outputs a motor position signal; a speed processor which receives the motor position signal and outputs a motor speed signal; and a movement position detector which detects the position of a movement section driven by the motor and outputs a movement position signal and provided with: a motor speed-control function which makes the speed command and the motor speed signal coincident by feedback control; and a motor position-control function which makes the position command and the movement position signal coincident by feedback control, wherein said position controller is provided with:
an observer which comprises a proportional operating means for performing proportional operation, an integration operating means for performing integration operation, an inertia model for inputting a torque command outputted from the speed control section, an adder which adds the output from the inertia model, the output from the proportional operating means, and the output from the integration operating means, an integrator which integrates the output from the adder and outputs an estimated speed, and a subtracter which inputs a differential signal, obtained by subtracting the estimated speed from the motor speed outputted from the speed processor, into the proportional operating means and the integration operating means and which produces the differential signal obtained by subtracting the estimated speed from the motor speed as an output; a high-pass filter which receives the output signal from the observer and outputs the output signal with its phase advanced; a low-pass filter in which the signal from the high-pass filter is inputted and which outputs the signal with only its high-frequency components eliminated; a coefficient multiplier which receives the signal from the observer and multiplies it by a torque corrective gain and outputs a torque correction signal; a section for torque correction evaluation which varies the torque corrective gain based on a position deviation signal that is the difference between the position command and the movement position signal; and a subtracter which subtracts the torque correction signal from the torque command to produce a new torque command, and further wherein;
in the section for torque correction evaluation, when the absolute value of the position deviation is greater than a reference value, the torque corrective gain is set to zero and when the absolute value of the position deviation changes to be equal to the reference value or below, the torque corrective gain is changed from zero to a constant value, or in the section for torque correction evaluation, when the absolute value of the position deviation signal is greater than a reference value, the torque corrective gain is set to zero and when the absolute value of the position deviation signal changes to be equal to the reference value or below, the torque corrective gain is continuously changed from zero to a constant value.
Furthermore, according to the present invention, a position controller of a movement section, which performs speed control based on a speed feedback signal obtained by differentiating a rotational displacement signal of a servomotor and which performs position control based on a position feedback signal from a position detector attached to a movement section driven by the servomotor or the rotating shaft of the servomotor, comprises: a correction evaluation section which sets a variable gain to zero when a position deviation between a position command and the position feedback signal is greater than a predetermined reference value, and which sets the variable gain to a constant positive number when the position deviation becomes equal to the reference value or below; a differential processing section which differentiates a value obtained by multiplying the position deviation by the variable gain; and a low-pass filter in which the output from this differential processing section is inputted and from which a speed command correction amount is outputted, wherein the speed command correction amount is added to a speed command base amount to produce the speed command of the servomotor
also, a position controller of a movement section, which performs speed control based on a speed feedback signal obtained by differentiating a rotational displacement signal of a servomotor and which performs position control based on a position feedback signal from a position detector attached to a movement section driven by the servomotor or the rotating shaft of the servomotor, comprises: a correction evaluation section which sets a variable gain to zero when a position deviation between a position command and the position feedback signal is greater than a predetermined reference value, and which sets the variable gain to a constant positive number when the position deviation becomes equal to the predetermined reference value or below; and a high-pass filter in which a value obtained by multiplying the position deviation by the variable gain is inputted and from which a speed correction amount is outputted, wherein the speed command correction amount is added to a speed command base amount to produce the speed command of the servomotor.